#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
import time

class MoveLineCommander(Node):
    def __init__(self):
        super().__init__('move_line_commander')

        self.pub_move_joint = self.create_publisher(Float64MultiArray, '/command_move_joint', 10)
        self.pub_move_line = self.create_publisher(Float64MultiArray, '/command_move_line', 10)

        # 预设指令
        # self.move_joint_cmd_init = [0,]

        # self.move_joint_cmds = [
        #     [0.140192, 0.567445, 0.247006, -2.116617, 0.438887, -0.342755, -0.108149],
        #     [0.140192, 0.547990, 0.214198, -2.116617, 0.496109, -0.301938, -0.108149],
        #     [0.139811, 0.542649, 0.174144, -2.116617, 0.479706, -0.291638, -0.108149],
        #     [0.140192, 0.520523, 0.125315, -2.116617, 0.477417, -0.229839, -0.108149],
        #     [0.139811, 0.514801, 0.083353, -2.116999, 0.487717, -0.219157, -0.118830],
        #     [0.139811, 0.506790, 0.032997, -2.116999, 0.451096, -0.184825, -0.118830],
        #     [0.139811, 0.506409, -0.001717, -2.116999, 0.451477, -0.180629, -0.118830],
        #     [0.137522, 0.509079, -0.039102, -2.116617, 0.443848, -0.187114, -0.108530],
        # ]

        # self.move_line_cmds = [
        #     [0.315448, 0.1082, 0.095691, 0.0331513, 0.92505, 0.117186, 0.359794],
        #     [0.3165, 0.0981854, 0.09828, 0.0673995, 0.928275, 0.110463, 0.348657],
        #     [0.319464, 0.086939, 0.098619, 0.0707685, 0.928608, 0.0958015, 0.351427],
        #     [0.319869, 0.074397, 0.101672, 0.0803043, 0.933963, 0.0849103, 0.337721],
        #     [0.321886, 0.061817, 0.102379, 0.0932874, 0.933654, 0.069634, 0.338731],
        #     [0.322653, 0.0483071, 0.103096, 0.0898777, 0.937317, 0.0527144, 0.332535],
        #     [0.323796, 0.0379144, 0.103035, 0.101273, 0.936468, 0.0404653, 0.333369],
        #     [0.325472, 0.0257291, 0.102855, 0.115844, 0.933564, 0.0264587, 0.338139],
        # ]

        self.move_joint_cmds = [
            [0.140192, 0.567445, 0.247006, -2.116617, 0.438887, -0.342755, -0.108149],
            [0.140192, 0.567445, 0.247006, -2.116617, 0.438887, -0.342755, -0.108149],
            [0.139811, 0.514801, 0.083353, -2.116999, 0.487717, -0.219157, -0.118830],
            [0.139811, 0.514801, 0.083353, -2.116999, 0.487717, -0.219157, -0.118830],
            [0.139811, 0.506790, 0.032997, -2.116999, 0.451096, -0.184825, -0.118830],
            [0.139811, 0.506790, 0.032997, -2.116999, 0.451096, -0.184825, -0.118830],
            [0.139811, 0.514801, 0.083353, -2.116999, 0.487717, -0.219157, -0.118830],
        ]


        self.move_line_cmds = [
            [0.315448, 0.1082, 0.095691, 0.0331513, 0.92505, 0.117186, 0.359794],
            [0.315448, 0.1082, 0.095691, 0.0331513, 0.92505, 0.117186, 0.359794],
            [0.321886, 0.061817, 0.102379, 0.0932874, 0.933654, 0.069634, 0.338731],
            [0.321886, 0.061817, 0.102379, 0.0932874, 0.933654, 0.069634, 0.338731],
            [0.322653, 0.0483071, 0.103096, 0.0898777, 0.937317, 0.0527144, 0.332535],
            [0.322653, 0.0483071, 0.103096, 0.0898777, 0.937317, 0.0527144, 0.332535],
            [0.321886, 0.061817, 0.102379, 0.0932874, 0.933654, 0.069634, 0.338731],
        ]


        self.execute_sequence()

    def wait_for_subscribers(self, timeout=1.0):
        start = time.time()
        while time.time() - start < timeout:
            if self.pub_move_joint.get_subscription_count() > 0:
                return True
            self.get_logger().info("Waiting move_joint_pianofor subscribers...")
            time.sleep(0.1)
        self.get_logger().warn("Timeout waiting for subscribers.")
        return False

    def execute_sequence(self):
        self.wait_for_subscribers(timeout=1.0)

        # 发布 joint 指令
        # msg_joint = Float64MultiArray()
        # msg_joint.data = self.move_joint_cmd
        # self.pub_move_joint.publish(msg_joint)
        # self.get_logger().info("Published move_joint_cmd.")
        # time.sleep(1.47)  # 等待机器人运动完毕

        # 发布多组 line 指令
        for i in range(len(self.move_joint_cmds)):
            msg_joint = Float64MultiArray()
            msg_joint.data = self.move_joint_cmds[i]
            self.pub_move_joint.publish(msg_joint)
            self.get_logger().info(f"Published move_joint_cmd {i+1}")
            time.sleep(0.2)  # 每条之间延时

            msg_line = Float64MultiArray()
            msg_line.data = self.move_line_cmds[i]
            self.pub_move_line.publish(msg_line)
            self.get_logger().info(f"Published move_line_cmd {i+1}")
            time.sleep(0.2)  # 每条之间延时

def main():
    rclpy.init()
    node = MoveLineCommander()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
